/**
 * @file dtw.hpp
 * @brief 基于Dijkstra算法的动态时间规整算法
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-10-24
 * 
 * @copyright Copyright (c) 2021  WHU-I2NAV
 * 
 */
#ifndef _DTW_HPP
#define _DTW_HPP

#include <vector>
#include <memory>
#include <algorithm>
#include <cmath>

class Node{
using NodePtr = std::shared_ptr<Node>;
public:
    int x;
    int y;
    double cost;
    NodePtr front = nullptr;

    ~Node(){}
    Node() = default;
    Node(int x, int y): x(x), y(y){}

    bool isEqual(const Node& other) const{
        return x == other.x && y == other.y;
    }
    bool operator()(const NodePtr& n1, const NodePtr& n2) const{
        return n1->cost > n2->cost;
    }
};

class DTW{
using NodePtr = std::shared_ptr<Node>;
private:
    std::vector<double> xList_;
    std::vector<double> yList_;
    std::vector<std::vector<int>> path_;
    enum MapStatus{
        UNVISITED = 0,
        EXPLORING,
        EXPLORED,
    };

public:
    ~DTW() = default;
    DTW() = default;
    DTW(std::vector<double>& xList, std::vector<double>& yList): xList_(xList), yList_(yList){};
    
    void setXList(std::vector<double>& xList);

    void setYList(std::vector<double>& yList);

    std::vector<std::vector<int>> getPath();

    bool search();
};

#endif 